#include "../../dsESC_Bram.X/Source/init.h"
#include "bootloader_init.h"
#include <p33FJ16GS504.h>

uReg32 boot_getflashword(UWord32 source)
{
    uReg32 Temp;

    TBLPAG = ((uReg32)source).Word.HW;
    Temp.Word.HW = __builtin_tblrdh(((uReg32)source).Word.LW);
    Temp.Word.LW = __builtin_tblrdl(((uReg32)source).Word.LW);

    return Temp;
}

void boot_erasepage(UWord32 source)
{
    NVMCON = PM_ROW_ERASE;
    TBLPAG = ((uReg32)source).Word.HW;
    __builtin_tblwtl(((uReg32)source).Word.LW, 0xFFFF);
    __builtin_write_NVM();
    while (NVMCONbits.WR);
    NVMCONbits.WREN = 0;
}

void boot_setflashword(UWord32 source, UWord32 value)
{
    __builtin_tblwth(((uReg32)source).Word.LW, ((uReg32)value).Word.HW);
    __builtin_tblwtl(((uReg32)source).Word.LW, ((uReg32)value).Word.LW);
}

void boot_writerow()
{
    NVMCON = PM_ROW_WRITE;
    __builtin_write_NVM();
    while (NVMCONbits.WR);
    NVMCONbits.WREN = 0;
}


/* EasyCODE ( 0
void InitPort(void) */
/* EasyCODE F */
void boot_initport()
{
    // PORTA: 43210
    TRISA = 0b00101;
    // A0:MC A1:MAA, A2:MB, A3:PWM1L, A4:PWM1H

    // PORTB: FEDCBA9876543210
    TRISB = 0b1000011111100110;
    // B0:Unused B1:OSC1 B2:OSC2 B3:DACOUT B4:Unused B5:TX B6:SCL B7:SDA
    // B8:PGED3 B9:MA B10:A3 B11:PWM3H B12:PWM3L B13:PWM2H B14:PWM2L B15:PGEC3

    //TRISBbits.TRISB1 = 0;		//OSC1
    //TRISBbits.TRISB2 = 0;		//OSC2
    //TRISBbits.TRISB8 = 0;		//PGED3
    //TRISBbits.TRISB15 = 0;	//PGEC3

    // PORTC: DCBA9876543210
    TRISC = 0b00011000000110;

    // C0:MCA C1:RX C2:CURRENT C3:Unused C4:GLED C5:OLED C6:RLED
    // C7:Unused C8:Unused C9:A2 C10:A1 C11:Unused C12:Unused C13:MBA

    // ADPC:   BA9876543210
    ADPCFG = 0b110111101010;
    //---------------------------------------------------------------------------------------------------------------------------
    /*ADPCFGbits.PCFG0=0;
    ADPCFGbits.PCFG1=1;
    ADPCFGbits.PCFG2=0;
    ADPCFGbits.PCFG3=1;
    ADPCFGbits.PCFG4=0;
    ADPCFGbits.PCFG5=1;
    ADPCFGbits.PCFG6=1;
    ADPCFGbits.PCFG7=1;
    ADPCFGbits.PCFG8=1;
    ADPCFGbits.PCFG9=0;//current
    ADPCFGbits.PCFG10=1;
    ADPCFGbits.PCFG11=1;*/

    //ADPCFG: A/D PORT CONFIGURATION REGISTER
    //PCFG<11:0>: A/D Port Configuration Control bits(1,2,3,4)
    //1 = Port pin in Digital mode, port read input enabled, A/D input multiplexor connected to AVSS
    //0 = Port pin in Analog mode, port read input disabled, A/D samples pin voltage

    // BEMF output low (when pin is set to output; low never changes)
    MAA=0;
    MBA=0;
    MCA=0;

    // Pullups off
    /*
     CNPU1 = 0;
    CNPU2 = 0;
     */

    /*
    _RP4R=0;
    _RP3R=0;
    _RP2R=0;
    _RP1R=0;
    _RP22R=0;
    _RP6R=0;
    _RP8R=0;
     * */

    // Peripheral select
    _RP5R = 0b000011; // U1TX @ B5
    _RP32R = 0b101010; // ACMP4 @ Internal
    _FLT1R = 32; // Fault1 pin to RP32
    _U1RXR = 0b010001; // U1RX @ RP17

    //ADDRESS = (A1 << 2) | (A2 << 1) | (A3);
}

/* EasyCODE ) */
/* EasyCODE ( 0
void InitCLOCK(void) */
/* EasyCODE F */
void boot_initclock()
{
/* Configure Oscillator to operate the device at 40Mhz
       Fosc= Fin*M/(N1*N2), Fcy=Fosc/2
       Fosc= 10*(32)/(2*2)=80Mhz for Fosc, Fcy = 40Mhz */
    /* Configure PLL prescaler, PLL postscaler, PLL divisor */
    PLLFBD=38; 				/* M = PLLFBD + 2 */
    CLKDIVbits.PLLPOST=0;   /* N1 = 2 */
    CLKDIVbits.PLLPRE=0;    /* N2 = 2 */

    __builtin_write_OSCCONH(0x03);			/* New Oscillator XT w/ PLL */
    __builtin_write_OSCCONL(OSCCON | 0x01);
    //OSCCONbits.OSWEN = 1;

    while (OSCCONbits.COSC != 0b011)
    {
        ;
    }
    while (OSCCONbits.LOCK != 1)
    {
        ;
    }
    ACLKCON = 0b1010011101000000;
    while (ACLKCONbits.APLLCK != 1);
    /* Wait for Auxiliary PLL to Lock */
    REFOCON = 0b1010000100000000;
}

/* EasyCODE ) */

/* EasyCODE F */
void boot_init_comm()
{
    _SI2C1IE = 0;
    I2C1MSK = 0b01100000;
    I2C1ADD = (1 << 3) | (A1 << 2) | (A2 << 1) | (A3 << 0);
    I2C1CON = 0b1001000001000000;
}
/* EasyCODE ) */

